#include "feature_track.h"
#include "feature_extractor.h"
#include "feature_matcher.h"
#include "numeric.h"
#include "sfm_ba.h"

#include <Eigen/Dense>
#include <boost/filesystem.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <ceres/local_parameterization.h>
#include <ceres/ceres.h>
#include <fstream>
#include <omp.h>
#include <set>

namespace ville3d {
FeatureTrack::FeatureTrack(float nnratio)
    : offset_(Eigen::Vector3d::Zero()),
      mfNNratio_(nnratio),
      curr_track_id_(0),
      overlap_size_(10),
      window_size_(30),
      track_builder_(new TracksBuilder),
      feature_matcher_(new FeatureMatcher()) {}

FeatureTrack::~FeatureTrack() {}

void FeatureTrack::SetOutPutDir(const std::string& output_dir) {
  output_dir_ = output_dir;
}

void FeatureTrack::SetCalibParam(int camera_id, double fx, double fy, double cx,
                                 double cy, double imag_width,
                                 double image_height) {
  image_width_[camera_id] = imag_width;
  image_height_[camera_id] = image_height;
  camera_k_[camera_id] << fx, 0, cx, 0, fy, cy, 0, 0, 1;
}

void FeatureTrack::TrackBuild() {
  LOG(INFO) << "begin matching ...";
  uint64_t neighbor_size = 10;
  std::vector<std::pair<IndexT, IndexT>> image_pairs;
  std::vector<Matches> matches_pairs;

  // match with neighbors;
  for (auto vit1 = views_.begin(); vit1 != views_.end(); vit1++) {
    IndexT index1 = vit1->first;
    for (auto vit2 = views_.begin(); vit2 != views_.end(); vit2++) {
      IndexT index2 = vit2->first;
      if (index1 == index2) continue;
      auto find = std::find_if(
          image_pairs.begin(), image_pairs.end(),
          [=](std::pair<IndexT, IndexT> pair) -> bool {
            return (pair.first == index1 && pair.second == index2) ||
                   (pair.second == index1 && pair.first == index2);
          });
      if (find != image_pairs.end()){
        continue;
      }

      std::shared_ptr<View> view1 = vit1->second;
      std::shared_ptr<View> view2 = vit2->second;
      if (IsCovisibility(view1, view2)) {
        // matches
        std::vector<cv::DMatch> matches;
        if (feature_matcher_->EpipolarMatchBetweenKeyFrames(
                view1, view2, matches, 0.8, 4.0)) {
          image_pairs.emplace_back(index1, index2);
          matches_pairs.push_back(matches);
        }
        LOG(INFO) << "matching between " << view1->view_id_ << " and "
                  << view2->view_id_ << " " << matches.size() << " matches";

        #if 0
        cv::Mat dump_image;
        cv::Mat image1 = cv::imread(view1->s_Img_path);
        cv::Mat image2 = cv::imread(view2->s_Img_path);
        cv::drawMatches(image1, view1->key_points_, image2, view2->key_points_,
                        matches, dump_image);
        std::string match_file =
            boost::filesystem::path("match")
                .append(view1->view_id_ + "-" + view2->view_id_ + ".jpg")
                .string();
        cv::imwrite(match_file, dump_image);
        #endif
      }
    }
  }

  LOG(INFO) << "match end";
  // build tracks
  track_builder_->Build(image_pairs, matches_pairs, tracks_);
  track_builder_->Filter(tracks_, 2);
  LOG(INFO) << "track build end";
}

void FeatureTrack::ReadImage(std::string image_file, std::string view_id,
                             int camera_id, const Eigen::Matrix4d& pose) {
  cv::Mat image = cv::imread(image_file);
  if (image.empty()) {
    return;
  }
  cv::Size size(image_width_[camera_id], image_height_[camera_id]);
  cv::resize(image, image, size);

  cv::Mat gray;
  cvtColor(image, gray, cv::COLOR_BGR2GRAY);
  //cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
  //clahe->apply(gray, gray);

  ViewID cur_view_id = view_id;
  if (offset_.norm() < 1e-1) {
    offset_ = pose.block<3, 1>(0, 3);
  }

  cv::Mat descriptors;
  std::vector<cv::KeyPoint> keypoints;
  FeatureExtractor feature_extractor(3000, 1.2, 8, 20, 7);
  feature_extractor.DoWork(gray, keypoints, descriptors);

  std::shared_ptr<View> view = std::make_shared<View>();
  view->height = image_height_[camera_id];
  view->width = image_width_[camera_id];
  view->intrinsic_ = camera_k_[camera_id];
  view->quats_ = Eigen::Quaterniond(pose.block<3, 3>(0, 0));
  view->trans_ = pose.block<3, 1>(0, 3) - offset_;
  view->prior_pose_ = pose;
  view->prior_pose_.block<3, 1>(0, 3) -= offset_;
  view->camera_id_ = camera_id;
  view->view_id_ = view_id;
  view->view_index = views_.size();
  view->key_points_ = keypoints;
  view->descriptors_ = descriptors;
  view->s_Img_path = image_file;
  views_[view->view_index] = view;
  view_indexs_.push_back(view->view_index);
}

double FeatureTrack::AngleBetweenRay(const Eigen::Matrix4d& pose_inv1,
                                     const Eigen::Matrix3d& intrinsic1,
                                     const Eigen::Matrix4d& pose_inv2,
                                     const Eigen::Matrix3d& intrinsic2,
                                     const Eigen::Vector2d& x1,
                                     const Eigen::Vector2d& x2) {
  // x = (u, v, 1.0)  // image coordinates
  // X = R.t() * K.inv() * x + C // Camera world point
  // ray = X - C = R.t() * K.inv() * x
  const Eigen::Vector3d ray1 = (pose_inv1.block<3, 3>(0, 0).transpose() *
                                intrinsic1.inverse() * x1.homogeneous())
                                   .normalized();
  const Eigen::Vector3d ray2 = (pose_inv2.block<3, 3>(0, 0).transpose() *
                                intrinsic2.inverse() * x2.homogeneous())
                                   .normalized();
  const double dotAngle = ray1.dot(ray2);
  return R2D(acos(clamp(dotAngle, -1.0 + 1.e-8, 1.0 - 1.e-8)));
}


void FeatureTrack::Triangularize() {
  // https://zhuanlan.zhihu.com/p/63179478
  if (tracks_.tracks.empty()) {
    return;
  }

  for (auto mit = tracks_.tracks.begin(); mit != tracks_.tracks.end(); mit++) {
    IndexT track_id = mit->first;
    std::vector<std::pair<IndexT, IndexT>> view_point_pairs = mit->second;

    std::size_t view_cnt = view_point_pairs.size();
    if (view_cnt < 2) {
        continue;
    }

    auto view_first = view_point_pairs[0];
    IndexT head_view_idx = view_first.first;
    std::shared_ptr<View> head_view = views_[head_view_idx];

    auto view_end = view_point_pairs[view_point_pairs.size() - 1];
    IndexT end_view_idx = view_end.first;
    std::shared_ptr<View> end_view = views_[end_view_idx];

    Eigen::Matrix4d pose1 = head_view->getInvPose();
    Eigen::Matrix4d pose2 = end_view->getInvPose();

    cv::KeyPoint x1 = head_view->key_points_[view_first.second];
    cv::KeyPoint x2 = end_view->key_points_[view_end.second];

    Eigen::Vector2d uv1(x1.pt.x, x1.pt.y);
    Eigen::Vector2d uv2(x2.pt.x, x2.pt.y);
    double angle = AngleBetweenRay(pose1, head_view->intrinsic_, pose2,
                                   end_view->intrinsic_, uv1, uv2);
    if (angle < 2.0) {
      continue;
    }

    Eigen::Vector3d X;
    if (view_cnt > 2) {
      std::vector<Eigen::Matrix4d> inv_poses;
      std::vector<Eigen::Vector3d> point_2d_normals;
      for (auto mit = view_point_pairs.begin(); mit != view_point_pairs.end();
           mit++) {
        IndexT view_idx = mit->first;
        std::shared_ptr<View> view = views_[view_idx];
        Eigen::Matrix4d inv_pose = view->getInvPose();
        inv_poses.push_back(inv_pose);

        Eigen::Matrix3d K = view->intrinsic_;
        cv::KeyPoint key_pt = view->key_points_[mit->second];
        Eigen::Vector3d point2d_normal =
            K.inverse() * Eigen::Vector3d(key_pt.pt.x, key_pt.pt.y, 1.0);
        point_2d_normals.push_back(point2d_normal);
      }

      if (!TriangulateNViewAlgebraic(point_2d_normals, inv_poses, X)) {
        continue;
      }
    } else {
      Triangulate2View(pose1, pose2, head_view->intrinsic_,
                       end_view->intrinsic_, uv1, uv2, X);
    }

    const Eigen::Vector2d residual_I = head_view->residual(X, uv1);
    const Eigen::Vector2d residual_J = end_view->residual(X, uv2);
    if (residual_I.norm() < 4.0 && residual_J.norm() < 4.0) {
      land_mark_points_[track_id] =
          std::make_shared<MapPoint>(X.head(3), track_id);
      land_mark_points_[track_id]->descriptor_ =
          head_view->descriptors_.row(view_first.second);
      land_mark_points_[track_id]->obs_num = view_point_pairs.size();
    }
  }
  LOG(INFO) << "Triangularize end";
}

void FeatureTrack::SupplementMatch() {
  int num = 0;
  for (auto mit = land_mark_points_.begin(); mit != land_mark_points_.end();
       mit++) {
    TrackID track_id = mit->first;
    std::shared_ptr<MapPoint> map_point = mit->second;

    for (auto vit = views_.begin(); vit != views_.end(); vit++) {
      IndexT view_index = vit->first;
      std::shared_ptr<View> view = vit->second;
      if (tracks_.image_tracks[view_index].find(track_id) !=
          tracks_.image_tracks[view_index].end()) {
        continue;
      }

      Eigen::Vector2d uv;
      if (view->project(map_point->position_, uv)) {
        std::vector<IndexT> feature_indexs =
            view->getFeaturesInArea(uv.x(), uv.y(), 10.0);

        int bestDist = 256;
        int best_match_idx = -1;
        for (IndexT feature_index : feature_indexs) {
          cv::Mat descript = view->descriptors_.row(feature_index);
          int dis = feature_matcher_->DescriptorDistance(
              descript, map_point->descriptor_);
          if (dis < 70 && dis < bestDist) {
            bestDist = dis;
            best_match_idx = feature_index;
          }
        }

        if (best_match_idx != -1) {
          tracks_.tracks[track_id].emplace_back(view_index, best_match_idx);
          tracks_.image_tracks[view_index].insert(track_id);
          tracks_.imageFeat_tracks[std::make_pair(view_index, best_match_idx)] =
              track_id;
          map_point->obs_num++;
          num++;
        }
      }
    }
  }
  LOG(INFO) << "supplement " << num << " matches";
}

bool FeatureTrack::TriangulateNViewAlgebraic(
    const std::vector<Eigen::Vector3d>& point_2d_normals,
    const std::vector<Eigen::Matrix4d>& inv_poses, Eigen::Vector3d& X) {
  assert(point_2d_normals.size() == inv_poses.size());

  Eigen::Matrix4d AtA = Eigen::Matrix4d::Zero();
  for (auto i = 0; i < point_2d_normals.size(); ++i) {
    const Eigen::Vector3d point_norm = point_2d_normals[i].normalized();
    const Eigen::Matrix<double, 3, 4> cost =
        inv_poses[i].block<3, 4>(0, 0) -
        point_norm * point_norm.transpose() * inv_poses[i].block<3, 4>(0, 0);
    
    AtA += cost.transpose() * cost;
  }

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigen_solver(AtA);
  Eigen::Vector4d val = eigen_solver.eigenvectors().col(0);
  X = val.hnormalized();
  return eigen_solver.info() == Eigen::Success;
}


bool FeatureTrack::Triangulate2View(const Eigen::Matrix4d& pose1,
                                    const Eigen::Matrix4d& pose2,
                                    const Eigen::Matrix3d& intrinsic1,
                                    const Eigen::Matrix3d& intrinisc2,
                                    const Eigen::Vector2d& uv1,
                                    const Eigen::Vector2d& uv2,
                                    Eigen::Vector3d& X) {
  Eigen::Matrix<double, 3, 4> P1 = intrinsic1 * pose1.block(0, 0, 3, 4);
  Eigen::Matrix<double, 3, 4> P2 = intrinisc2 * pose2.block(0, 0, 3, 4);
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(4, 4);
  H.row(0) = uv1.y() * P1.row(2) - P1.row(1);
  H.row(1) = P1.row(0) - uv1.x() * P1.row(2);
  H.row(2) = uv2.y() * P2.row(2) - P2.row(1);
  H.row(3) = P2.row(0) - uv2.x() * P2.row(2);

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix4d V = svd.matrixV();
  Eigen::Vector4d val = V.block(0, 3, 4, 1);
  X = val.hnormalized();
  return true;
}

bool FeatureTrack::IsCovisibility(const std::shared_ptr<View>& view0,
                                  const std::shared_ptr<View>& view1) {
  //������һ����ǰ��50�׵ĵ��Ƿ�����һ����ͼ�б�����,����Ȳ��ܳ���150��
  Eigen::Vector2d image_center(view0->width / 2.0, view0->height / 2.0);
  Eigen::Vector3d p3d =
      50.0 * view0->intrinsic_.inverse() * image_center.homogeneous();
  
  //���õ�ת����view1��
  Eigen::Matrix4d relative_pose = view1->getInvPose() * view0->getPose();
  Eigen::Vector3d p3d_in_view1 =
      relative_pose.block<3, 4>(0, 0) * p3d.homogeneous();

  //Ҫ�����ǰ��,����Ȳ��ܳ���150
  if (p3d_in_view1.z() < 0 || p3d_in_view1.z() > 150.0) {
    return false;
  }

  //ͶӰ�����ƽ����,���ܳ���ͼ��ߴ�
  Eigen::Vector3d uvw = view1->intrinsic_ * p3d_in_view1;
  Eigen::Vector2d uv = uvw.hnormalized();
  if (uv.x() < 0 || uv.x() > view1->width || uv.y() < 0 ||
      uv.y() > view1->height) {
    return false;
  }
  return true;
}

void FeatureTrack::DumpTrackPoint() {
  // dump;
   for (auto mit = land_mark_points_.begin(); mit != land_mark_points_.end();
        mit++) {
     TrackID trackID = mit->first;
     Eigen::Vector3d X = mit->second->position_;
     std::vector<std::pair<IndexT, IndexT>> image_feat_map =
         tracks_.tracks[trackID];
     int match_view_count = image_feat_map.size();
     cv::Mat dump_image(image_height_[0] * match_view_count, image_width_[0],
                        CV_8UC3, cv::Scalar(0, 0, 0));

     std::size_t img_idx = 0;
     for (auto fit = image_feat_map.begin(); fit != image_feat_map.end();
          fit++) {
       IndexT image_idx = fit->first;
       IndexT feat_idx = fit->second;

       std::shared_ptr<View> view = views_[image_idx];
       cv::Mat image = cv::imread(view->s_Img_path);
       int font_face = cv::FONT_HERSHEY_PLAIN;
       double font_scale = 2.0;
       int thickness = 1;
       int baseline;
       cv::Size text_size =
           cv::getTextSize(view->view_id_, font_face, font_scale, thickness, &baseline);
       cv::putText(image, view->view_id_, cv::Point2f(10.0, 20.0), font_face,
                   font_scale, cv::Scalar(0, 0, 255), thickness);

       cv::KeyPoint keypoint = view->key_points_[feat_idx];
       cv::circle(image, cv::Point(keypoint.pt.x, keypoint.pt.y), 3,
                  cv::Scalar(0, 0, 255), 3);
       Eigen::Vector2d uv;
       if (view->project(X, uv)) {
         cv::circle(image, cv::Point(uv.x(), uv.y()), 3, cv::Scalar(0, 255, 0),
                    2);
       }

       cv::Rect rect(0, img_idx * image_height_[0], image_width_[0],
                     image_height_[0]);
       image.copyTo(dump_image(rect));
       img_idx++;
     }
     std::string track_dir =
         boost::filesystem::path(output_dir_).append("track").string();
     if (!boost::filesystem::exists(track_dir)) {
       boost::filesystem::create_directories(track_dir);
     }

     std::string image_name = boost::filesystem::path(track_dir)
                                  .append(std::to_string(trackID) + ".jpg")
                                  .string();
     cv::imwrite(image_name, dump_image);
   }
}

bool FeatureTrack::BadTrackRejector(double dPrecision,
                                    std::size_t count /*= 0*/) {
  LOG(INFO) << "begin to remove outlier ...";
  const std::size_t nbOutliers_residualErr =
      RemoveOutliers_PixelResidualError(dPrecision, 2);
  LOG(INFO) << "remove outlier pixel residual error!";
  const std::size_t nbOutliers_angleErr = RemoveOutliers_AngleError(2.0);
  LOG(INFO) << "remove outlier angle error!";
  return (nbOutliers_residualErr + nbOutliers_angleErr) > count;
}

std::size_t FeatureTrack::RemoveOutliers_PixelResidualError(
    const double dThresholdPixel, const unsigned int minTrackLength /*= 2*/) {
  std::size_t outlier_count = 0;
  auto land_mark_it = land_mark_points_.begin();
  while (land_mark_it != land_mark_points_.end()) {
    Eigen::Vector3d land_mark_point = land_mark_it->second->position_;
    TrackID track_id = land_mark_it->first;

    std::vector<std::pair<IndexT, IndexT>> img_feature_map =
        tracks_.tracks[track_id];

    for (int i = 0; i < img_feature_map.size(); i++) {
      IndexT view_index = img_feature_map[i].first;
      std::shared_ptr<View> view = views_[view_index];

      cv::KeyPoint key_point = view->key_points_[img_feature_map[i].second];
      Eigen::Vector2d point2d(key_point.pt.x, key_point.pt.y);
      if (!view->cheiralityTest(point2d, land_mark_point)) {
        tracks_.remove_image_from_track(track_id, view_index);
        outlier_count++;
      } else {
        Eigen::Vector2d uv_i;
        if (view->project(land_mark_point, uv_i)) {
          double residual = (uv_i - point2d).norm();
          if (residual > dThresholdPixel) {
            tracks_.remove_image_from_track(track_id, view_index);
            outlier_count++;
          }
        } else {
          tracks_.remove_image_from_track(track_id, view_index);
          outlier_count++;
        }
      }
    }

    if (img_feature_map.empty() || img_feature_map.size() < minTrackLength) {
      land_mark_it = land_mark_points_.erase(land_mark_it);
    } else {
      land_mark_it++;
    }
  }
  return outlier_count;

}

std::size_t FeatureTrack::RemoveOutliers_AngleError(
    const double dMinAcceptedAngle) {
  std::size_t removedTrack_count = 0;
  auto land_mark_it = land_mark_points_.begin();
  while (land_mark_it != land_mark_points_.end()) {
    Eigen::Vector3d land_mark_point = land_mark_it->second->position_;
    TrackID track_id = land_mark_it->first;

    double max_angle = 0.0;
    std::vector<std::pair<IndexT, IndexT>>& img_feature_map = tracks_.tracks[track_id];
    for (std::size_t i = 0; i < img_feature_map.size(); i++) {
      std::shared_ptr<View> view1 = views_[img_feature_map[i].first];
      cv::KeyPoint kp1 = view1->key_points_[img_feature_map[i].second];

      for (std::size_t j = i + 1; j < img_feature_map.size(); j++) {
        std::shared_ptr<View> view2 = views_[img_feature_map[j].first];
        cv::KeyPoint kp2 = view2->key_points_[img_feature_map[j].second];

        const double angle = AngleBetweenRay(
            view1->getInvPose(), view1->intrinsic_, view2->getInvPose(),
            view2->intrinsic_, Eigen::Vector2d(kp1.pt.x, kp1.pt.y),
            Eigen::Vector2d(kp2.pt.x, kp2.pt.y));
        max_angle = std::max(angle, max_angle);
      }
    }

    if (max_angle < dMinAcceptedAngle) {
      tracks_.remove_track(track_id);
      land_mark_it = land_mark_points_.erase(land_mark_it);
      removedTrack_count++;
    } else
      land_mark_it++;
  }
  return removedTrack_count;
}

bool FeatureTrack::EraseUnstablePosesAndObservations(
    const int min_points_per_pose /*= 6*/,
    const int min_points_per_landmark /*= 2*/) {
  EraseObservationsWithMissingPoses(min_points_per_landmark);
  // Then iteratively remove orphan poses & observations
  IndexT remove_iteration = 0;
  bool bRemovedContent = false;
  do {
    bRemovedContent = false;
    if (EraseMissingPoses(min_points_per_pose)) {
      bRemovedContent =
          EraseObservationsWithMissingPoses(min_points_per_landmark);
    }
    remove_iteration += bRemovedContent ? 1 : 0;
  } while (bRemovedContent);
  LOG(INFO) << "erase unstable poses and observations!";
  return remove_iteration > 0;
}

bool FeatureTrack::EraseObservationsWithMissingPoses(
    const IndexT min_points_per_landmark /*= 2*/) {
  //���landmark�Ĺ۲���С��min_points_per_landmark����Ѹ�landmark��track��ȥ��
  std::vector<TrackID> tracks_to_remove;
  for (auto land_mark_it = land_mark_points_.begin();
       land_mark_it != land_mark_points_.end(); land_mark_it++) {
    TrackID track_id = land_mark_it->first;
    std::vector<std::pair<IndexT, IndexT>> img_feats = tracks_.tracks[track_id];
    if (img_feats.size() < min_points_per_landmark) {
      tracks_to_remove.push_back(track_id);
    }
  }

  for (auto track_id : tracks_to_remove) {
    land_mark_points_.erase(track_id);
    tracks_.remove_track(track_id);
  }
  return true;
}

bool FeatureTrack::EraseMissingPoses(const IndexT min_points_per_pose /*= 6*/) {
  //���ͼ���landmark����min_points_per_pose�� �ͰѸ�ͼ���track��ȥ��
  std::vector<IndexT> img_to_remove;
  for (auto mit = tracks_.image_tracks.begin();
       mit != tracks_.image_tracks.end(); mit++) {
    //�����landmark
    int num_track_valid = 0;
    for (auto track_id : mit->second) {
      if (land_mark_points_.find(track_id) != land_mark_points_.end()) {
        num_track_valid++;
      }
    }
    if (num_track_valid != 0 && num_track_valid < min_points_per_pose) {
      img_to_remove.push_back(mit->first);
      LOG(INFO) << "remove image : " << views_[mit->first]->view_id_;
    }
  }
  for (auto image_index : img_to_remove) {
    tracks_.remove_image(image_index);
  }
  return img_to_remove.size() > 0;
}

void FeatureTrack::Optimize() {
 // do {
    /* EraseUnstablePosesAndObservations();*/
    BundleAdjustment();
 // } while (BadTrackRejector(4.0, 50));
}

void FeatureTrack::BundleAdjustment() {
  ceres::Problem problem;
  ceres::LossFunction* loss_function = new ceres::CauchyLoss(1.0);

  //land_mark constraint
  for (auto mit = land_mark_points_.begin(); mit != land_mark_points_.end();
       mit++) {
    IndexT track_id = mit->first;
    for (auto vit = tracks_.tracks[track_id].begin();
         vit != tracks_.tracks[track_id].end(); vit++) {
      IndexT view_index = vit->first;
      std::shared_ptr<View> view = views_[view_index];

      cv::KeyPoint key_point = view->key_points_[vit->second];
      Eigen::Vector2d point2d(key_point.pt.x, key_point.pt.y);

      ceres::CostFunction* cost_function = PoseGraph3dErrorTerm::Create(
          point2d, view->intrinsic_, mit->second->obs_num);
      problem.AddResidualBlock(
          cost_function, loss_function, view->quats_.coeffs().data(),
          view->trans_.data(), mit->second->position_.data());
    }
  }
  //relative pose constraint
  for (std::size_t i = 0; i < view_indexs_.size() - 1; i++) {
    std::shared_ptr<View> view_0 = views_[view_indexs_[i]];
    std::shared_ptr<View> view_1 = views_[view_indexs_[i + 1]];
    Eigen::Matrix4d relative_pose =
        view_0->prior_pose_.inverse() * view_1->prior_pose_;
    
    ceres::CostFunction* cost_function =
        PriorDeltaErrorTerm::Create(relative_pose, 100.0, 100.0);
    problem.AddResidualBlock(
        cost_function, loss_function, view_0->quats_.coeffs().data(),
        view_0->trans_.data(), view_1->quats_.coeffs().data(),
        view_1->trans_.data());
  }

  // prior pose constraint
  std::vector<double> var_vec = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0};
  for (std::size_t i = 0; i < view_indexs_.size(); i++) {
    IndexT view_index = view_indexs_[i];
    std::shared_ptr<View> view = views_[view_index];
    Eigen::Matrix4d proir_pose = view->prior_pose_;

    ceres::CostFunction* cost_function =
        GlobalPriorPoseFactor::Create(proir_pose, var_vec);
    problem.AddResidualBlock(cost_function, loss_function,
                             view->quats_.coeffs().data(), view->trans_.data());
  }

  for (std::size_t i = 0; i < view_indexs_.size(); i++) {
    std::shared_ptr<View> view = views_[view_indexs_[i]];
    ceres::LocalParameterization* parmeterization =
        new ceres::EigenQuaternionParameterization;
    problem.SetParameterization(view->quats_.coeffs().data(), parmeterization);
  }

  ceres::Solver::Options options;
  options.max_num_iterations = 100;
  options.linear_solver_type = ceres::DENSE_SCHUR;  // DENSE_SCHUR
  options.trust_region_strategy_type =
      ceres::DOGLEG;  // ceres::LEVENBERG_MARQUARDT;
  options.use_explicit_schur_complement = true;
  options.minimizer_progress_to_stdout = true;
  options.num_threads = 6;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  if (!summary.IsSolutionUsable()) {
    LOG(INFO) << "Bundle Adjustment failed.";
  } else {
    LOG(INFO) << summary.BriefReport();
    LOG(INFO) << "Bundle Adjustment succeed.";
  }
}

void FeatureTrack::GenerateSfMReport(const std::string& report_name) {
  std::string report_file =
      boost::filesystem::path(output_dir_).append(report_name).string();
  std::ofstream residual_ofs(report_file);
  for (auto track_it = tracks_.image_tracks.begin();
       track_it != tracks_.image_tracks.end(); track_it++) {
    IndexT view_idx = track_it->first;
    std::shared_ptr<View> view = views_[view_idx];
    std::set<TrackID> track_ids = track_it->second;

    int num = 0;
    double residual = 0.0;
    for (auto track_it = track_ids.begin(); track_it != track_ids.end();
         track_it++) {
      TrackID track_id = *track_it;
      if (land_mark_points_.find(track_id) != land_mark_points_.end()) {
        Eigen::Vector2d uv;
        if (view->project(land_mark_points_[track_id]->position_, uv)) {
          IndexT feat_index = -1;
          if (tracks_.search_feat_by_track_image_id(track_id, view_idx,
                                                    feat_index)) {
            cv::KeyPoint key_pt = view->key_points_[feat_index];
            double diff =
                (uv - Eigen::Vector2d(key_pt.pt.x, key_pt.pt.y)).squaredNorm();
            residual += diff;
            num++;
          }
        }
      }
    }
    if (num > 0) {
      residual = std::sqrt(residual / num);
    }
    residual_ofs << std::fixed << "view_idx : " << view->view_id_
                 << " residual_error : " << residual << std::endl;
  }
  residual_ofs.close();
}

void FeatureTrack::DumpProject() {
  std::ofstream ofs("tracks.txt");
  for (auto mit = tracks_.tracks.begin(); mit != tracks_.tracks.end(); mit++) {
    TrackID track_id = mit->first;
    if (land_mark_points_.find(track_id) != land_mark_points_.end()) {
      ofs << "track_id : " << mit->first << std::endl;
      for (auto vit = mit->second.begin(); vit != mit->second.end(); vit++) {
        std::shared_ptr<View> view = views_[vit->first];
        ofs << "image_feat : " << vit->first << ","
            << view->key_points_[vit->second].pt.x << ","
            << view->key_points_[vit->second].pt.y << std::endl;
      }
      ofs << std::endl;
    }
  }
  ofs.close();

  std::ofstream ofs1("image_track.txt");
  for (auto mit = tracks_.image_tracks.begin();
       mit != tracks_.image_tracks.end(); mit++) {
    IndexT image_id = mit->first;
    ofs1 << "view_id : " << views_[image_id]->view_id_ << std::endl;
    std::set<TrackID> track_ids = mit->second;
    for (auto track_id : track_ids) {
      if (land_mark_points_.find(track_id) != land_mark_points_.end()) {
        ofs1 << "track_id : " << track_id << std::endl;
      }
    }
    ofs1 << std::endl;
  }
  ofs1.close();

  for (auto track_it = tracks_.image_tracks.begin();
       track_it != tracks_.image_tracks.end(); track_it++) {
    IndexT view_idx = track_it->first;
    std::shared_ptr<View> view = views_[view_idx];
    cv::Mat image = cv::imread(view->s_Img_path);
    cv::Mat dump_image = image.clone();
 
    for (auto mit = land_mark_points_.begin(); mit != land_mark_points_.end();
         mit++) {
      Eigen::Vector2d point2d;
      TrackID track_id = mit->first;
      Eigen::Vector3d land_mark = mit->second->position_;
     /* if (track_it->second.find(track_id) == track_it->second.end()) {
        continue;
      }*/

      if (view->project(land_mark, point2d)) {
        cv::circle(dump_image, cv::Point(point2d.x(), point2d.y()), 3.0,
                   cv::Scalar(0, 255, 0));

        std::string text = std::to_string(track_id);
        int font_face = cv::FONT_HERSHEY_PLAIN;
        double font_scale = 1.0;
        int thickness = 1;
        int baseline;
        cv::Size text_size =
            cv::getTextSize(text, font_face, font_scale, thickness, &baseline);

        cv::putText(dump_image, text, cv::Point2f(point2d.x(), point2d.y()),
                    font_face, font_scale, cv::Scalar(0, 255, 255), thickness);
      }
    }
    
    for (cv::KeyPoint key_point : view->key_points_) {
      cv::circle(dump_image, cv::Point(key_point.pt.x, key_point.pt.y), 1.0,
                 cv::Scalar(0, 0, 255));
    }

    std::string project_dir = boost::filesystem ::path(output_dir_)
                                  .append("project")
                                  .append(std::to_string(view->camera_id_))
                                  .string();
    if (!boost::filesystem::exists(project_dir)) {
      boost::filesystem::create_directories(project_dir);
    }

    std::string image_name = boost::filesystem::path(project_dir)
                                 .append(view->view_id_ + ".jpg")
                                 .string();
    cv::imwrite(image_name, dump_image);
  }
  LOG(INFO) << "dump projected image end";
}

void FeatureTrack::SaveOdomToFile() {
  std::string track_file =
      boost::filesystem::path(output_dir_).append("images.txt").string();
  std::ofstream ofs(track_file);
  for (auto vit = views_.begin(); vit != views_.end(); vit++) {
    ViewID view_id = vit->second->view_id_;
    Eigen::Quaterniond quat = vit->second->quats_;
    Eigen::Vector3d translation = vit->second->trans_ /* + offset_*/;
    Eigen::Matrix3d rotation = quat.toRotationMatrix();
    ofs << std::fixed << view_id << " " << rotation(0, 0) << " "
        << rotation(0, 1) << " " << rotation(0, 2) << " "
        << translation(0) /*+ offset_.x() */<< " " << rotation(1, 0) << " "
        << rotation(1, 1) << " " << rotation(1, 2) << " "
        << translation(1) /*+ offset_.y() */<< " " << rotation(2, 0) << " "
        << rotation(2, 1) << " " << rotation(2, 2) << " "
        << translation(2) /*+ offset_.z() */<< std::endl;
  }
  ofs.close();
  LOG(INFO) << "save odom text end";
}

void FeatureTrack::SaveColmapToPath() {
  std::string colmap_path =
      boost::filesystem::path(output_dir_).append("colmap").string();
  std::string sparse_path =
      boost::filesystem::path(colmap_path).append("sparse").string();
  if (!boost::filesystem::exists(sparse_path)) {
    boost::filesystem::create_directories(sparse_path);
  }

  std::string camera_path =
      boost::filesystem::path(sparse_path).append("cameras.txt").string();
  std::ofstream camera_ofs(camera_path);
  for (auto mit = camera_k_.begin(); mit != camera_k_.end(); mit++) {
    camera_ofs << mit->first << " PINHOLE " << image_width_[mit->first] << " "
               << image_height_[mit->first] << " "
               << camera_k_[mit->first](0, 0) << " "
               << camera_k_[mit->first](1, 1) << " "
               << camera_k_[mit->first](0, 2) << " "
               << camera_k_[mit->first](1, 2)
               << std::endl;
  }
  camera_ofs.close();

  std::string image_name_path =
      boost::filesystem::path(sparse_path).append("images.txt").string();
  std::ofstream image_ofs(image_name_path);
  for (auto vit = views_.begin(); vit != views_.end(); vit++) {
    IndexT image_index = vit->first;
    std::shared_ptr<View> view = vit->second;
    Eigen::Quaterniond quats = view->quats_;
    Eigen::Vector3d trans = view->trans_;
    Eigen::Quaterniond inv_quats = quats.conjugate();
    Eigen::Vector3d inv_trans = -inv_quats.toRotationMatrix() * trans;
    image_ofs << image_index << " " << inv_quats.w() << " " << inv_quats.x()
              << " " << inv_quats.y() << " " << inv_quats.z() << " "
              << inv_trans.x() << " " << inv_trans.y() << " " << inv_trans.z()
              << " " << view->camera_id_ << " " << view->view_id_ + ".png"
              << std::endl;

    KeyPoints keypoints = view->key_points_;
    if (!keypoints.empty()) {
      for (std::size_t feat_idx = 0; feat_idx < keypoints.size(); feat_idx++) {
        IndexT land_mark_index = -1;
        if (tracks_.imageFeat_tracks.find(std::make_pair(
                image_index, feat_idx)) != tracks_.imageFeat_tracks.end()) {
          IndexT track_id = tracks_.imageFeat_tracks[{image_index, feat_idx}];
          if (land_mark_points_.find(track_id) != land_mark_points_.end()) {
            land_mark_index = track_id;
          }
        }
        image_ofs << std::fixed << keypoints[feat_idx].pt.x << " "
                  << keypoints[feat_idx].pt.y << " " << land_mark_index << " ";
      }
    } else {
      image_ofs << "#";
    }
    image_ofs << std::endl;
  }
  image_ofs.close();

  std::string point_path =
      boost::filesystem::path(sparse_path).append("points3D.txt").string();
  std::ofstream point_ofs(point_path);
  for (auto mit = land_mark_points_.begin(); mit != land_mark_points_.end();
       mit++) {
    IndexT track_id = mit->first;
    Eigen::Vector3d land_mark = mit->second->position_;
    point_ofs << std::fixed << track_id << " " << land_mark.x() << " "
              << land_mark.y() << " " << land_mark.z() << " 255 255 255 1.0 ";
    std::vector<std::pair<IndexT, IndexT>> img_features = tracks_.tracks[track_id];
    for (auto vit = img_features.begin(); vit != img_features.end(); vit++) {
      point_ofs << vit->first << " " << vit->second << " ";
    }
    point_ofs << std::endl;
  }
  point_ofs.close();

  std::string images_path =
      boost::filesystem::path(colmap_path).append("images").string();
  if (!boost::filesystem::exists(images_path)) {
    boost::filesystem::create_directories(images_path);
  }
  for (std::size_t i = 0; i < view_indexs_.size(); i++) {
    IndexT view_index = view_indexs_[i];
    std::shared_ptr<View> view = views_[view_index];
    cv::Mat image = cv::imread(view->s_Img_path);
    std::string image_name = boost::filesystem::path(images_path)
                                 .append(view->view_id_ + ".png")
                                 .string();
    cv::imwrite(image_name, image);
  }
  LOG(INFO) << "dump colmap format end";
}

void FeatureTrack::SaveMapPointToFile(const std::string& cloud_file) {
  std::string map_point_file =
      boost::filesystem::path(output_dir_).append(cloud_file).string();

  std::ofstream ofs(map_point_file);
  for (auto mit = land_mark_points_.begin(); mit != land_mark_points_.end();
       mit++) {
    Eigen::Vector3d map_point = mit->second->position_ /*+ offset_*/;
    ofs << std::fixed << map_point.x() << "," << map_point.y() << ","
        << map_point.z() << "," << mit->first << std::endl;
  }
  ofs.close();
  LOG(INFO) << "save map point end";
}

}  // namespace ville3d